
/////////////////////////////////////
// PIN CONFIGURATION
/////////////////////////////////////
// Motors
const int dirR_pin = 12;
const int dirL_pin = 13;  
const int speedR_pin = 3;
const int speedL_pin = 11; 
// RGB led
const int red_pin=2;
const int green_pin=4;
const int blue_pin=7;
// Buzzer
const int buzzer_pin = 10;
// Eyes
const int eyeR_pin = A3;
const int eyeL_pin = A2;
/////////////////////////////////////

const byte FORWARD = 0;
const byte REVERSE = 1;
const byte LEFT = 2;
const byte RIGHT = 3;

/////////////////////////////////////
// State variables
/////////////////////////////////////
byte robotDirection = 0;
byte robotVelocity = 0;
/////////////////////////////////////

boolean firstLoop = true;

void setup()
{  
  //Serial.begin(9600);
  
  pinMode (dirR_pin, OUTPUT);
  pinMode (dirL_pin, OUTPUT);
  pinMode (speedR_pin, OUTPUT);
  pinMode (speedL_pin, OUTPUT);
  
  pinMode(green_pin,OUTPUT);
  pinMode(blue_pin,OUTPUT);
  pinMode(red_pin,OUTPUT);

  pinMode(buzzer_pin,OUTPUT);
  
  pinMode(eyeL_pin,INPUT);
  pinMode(eyeR_pin,INPUT);
  
  rgb_off();
  noTone(buzzer_pin);
  stopMotors();
  
  
}

void loop()
{
  if(firstLoop)
  { 
    police();
  
    testMoving();
    stopMotors();
         
    rgb_multi();
    firstLoop = false;
  }
  else //next loops
  {
    //followLight();  
    stopMotors();
  }  
}

/////////////////////////////////////
// MOVING AROUND
/////////////////////////////////////
void stopMoving()
{
  digitalWrite(speedL_pin, LOW);  
  digitalWrite(speedL_pin, LOW); 
  robotVelocity = 0;
}

void stopMotors()
{
  stopMoving();
}

void move(byte dir)
{
  move(dir, 255);
}

void move(byte dir, byte vel)
{
  move(dir, vel, -1);
}

void move(byte dir, byte vel, int time)
{
  if(vel>255)
    vel = 255;
  if(vel<150)
    vel = 150;  
  
  robotDirection = dir;
  robotVelocity = vel;  

  
  switch ( dir ) 
  {
    case FORWARD:
      digitalWrite (dirR_pin, HIGH); 
      digitalWrite (dirL_pin, LOW);         
      break;
    case REVERSE:
      digitalWrite (dirR_pin, LOW); 
      digitalWrite (dirL_pin, HIGH);        
      break;
    case LEFT:
      digitalWrite (dirR_pin, HIGH); 
      digitalWrite (dirL_pin, HIGH);         
      break;
    case RIGHT:
      digitalWrite (dirR_pin, LOW); 
      digitalWrite (dirL_pin, LOW);         
      break;
    default:
      break;
  }    
 
  analogWrite (speedL_pin, vel);
  analogWrite (speedR_pin, vel);
  
  
  if(time>0)
  {
    delay (time);
    digitalWrite(speedL_pin, LOW);  
    digitalWrite(speedR_pin, LOW); 
  }
}

void testMoving()
{
  //Forward
  move(FORWARD,255, 1500);
  delay(1000);
    
  //Rewind
  move(REVERSE,255, 1500);
  delay(1000);

  //Left
  move(LEFT,255, 1000);
  delay(1000);
  
  //Right
  move(RIGHT,255, 1000);
  delay(1000);
  
  //Forward increasing and decreasing    
  for (int j = 150; j < 255; j += 10) 
  {
     move(FORWARD,j, 100);
  }
  for (int j = 255; j >= 150; j -= 10) 
  {
     move(FORWARD,j, 100);
  }
  stopMotors();
}
/////////////////////////////////////


/////////////////////////////////////
// RGB LED and BUZZER
/////////////////////////////////////
void rgb_off()
{
  digitalWrite(red_pin,LOW);
  digitalWrite(green_pin,LOW);
  digitalWrite(blue_pin,LOW);
}

void rgb_white()
{
  digitalWrite(red_pin,HIGH);
  digitalWrite(green_pin,HIGH);
  digitalWrite(blue_pin,HIGH);
}

void rgb_red()
{
  digitalWrite(red_pin,HIGH);
  digitalWrite(green_pin,LOW);
  digitalWrite(blue_pin,LOW);
}

void rgb_green()
{
  digitalWrite(red_pin,LOW);
  digitalWrite(green_pin,HIGH);
  digitalWrite(blue_pin,LOW);
}

void rgb_blue()
{
  digitalWrite(red_pin,LOW);
  digitalWrite(green_pin, LOW);
  digitalWrite(blue_pin,HIGH);
}

void rgb_purple()
{
  digitalWrite(red_pin,HIGH);
  digitalWrite(green_pin,LOW);
  digitalWrite(blue_pin,HIGH);
}

void rgb_bho()
{
  digitalWrite(red_pin,HIGH);
  digitalWrite(green_pin,HIGH);
  digitalWrite(blue_pin,LOW);
}

void rgb_yellow()
{
  digitalWrite(red_pin,LOW);
  digitalWrite(green_pin,HIGH);
  digitalWrite(blue_pin,HIGH);
}

void rgb_multi()
{
  rgb_blue();
  delay(250);
  rgb_yellow();
  delay(250);
  rgb_purple();
  delay(250);
  rgb_bho();
  delay(250);
  rgb_red();
  delay(250);
  rgb_green();
  delay(250);
  rgb_white();
  delay(250);
  rgb_off();
  delay(250);
}

void police()
{
  noTone(buzzer_pin);
  delay(100);
  for(int i=1; i<3; i++)
  {
    rgb_red();
    tone(buzzer_pin, 400);
    delay(700);
    noTone(buzzer_pin);
    rgb_off();
    delay(20);
    rgb_blue();
    tone(buzzer_pin, 600);
    delay(700);
    noTone(buzzer_pin);
    rgb_off();
    delay(20);     
  }
  noTone(buzzer_pin); 
}
/////////////////////////////////////

/////////////////////////////////////
// EYES 
/////////////////////////////////////
int deltaLight()
{
    int eyeR_light = analogRead(eyeR_pin);
    int eyeL_light = analogRead(eyeL_pin);
    
    int delta = eyeR_light - eyeL_light;

/*  
    Serial.print(eyeR_light);
    Serial.print("-");
    Serial.println(eyeL_light);   
    Serial.print(" -> ");
    Serial.println(delta); 
*/    
    return delta;
}

void followLight()
{
  int delta = deltaLight();
  byte lightDirection;
  if(delta>0)
  {
    lightDirection = RIGHT;
  }
  if(delta<0)
  {
    lightDirection = LEFT;
    delta=-delta;
  }
  
  //led
  if(delta>3)
    if(lightDirection==LEFT)
      rgb_yellow();
    else
      rgb_green();
  else
    rgb_off();

  if(delta>6)
    move(lightDirection, 255, 200);
  else
  if(delta>4)
    move(lightDirection, 255, 75);  
    
  delay(100);  
}
/////////////////////////////////////
